#include "bsp_inc.h"
#include "osc_api.h"
#include "osc_task.h"
#include "osc_application.h"

typedef struct AxisDemo_TAttrTag   AxisDemo_TAttr;
struct AxisDemo_TAttrTag
{
    Bsp_THandle handle;
    Bsp_TAxisAcceData acce_data[10];
    Bsp_TAxisGyroData gyro_data[10];
    Bsp_TAxisAngleData angle_data[10];
};

static  AxisDemo_TAttr g_axis_demo_attr_;

static Osc_TTask g_axis_demo_task_ = {0};
static Int8 g_axis_demo_task_stack_mem_[512] __attribute__((aligned(8))) = {0};
static const Osc_TTaskAttr g_axis_demo_task_attr_ = {
    .name = "AxisTask",
    .priority = Osc_kTaskPriorityNormal,
    .stack_mem = g_axis_demo_task_stack_mem_,
    .stack_size = sizeof(g_axis_demo_task_stack_mem_),
};

static void AxisDemo_TaskOnStart(Osc_TTask *self)
{
    AxisDemo_TAttr* attr = &g_axis_demo_attr_;

    attr->handle = Bsp_AxisCreate(0);
    Bsp_AxisSetDataDepth(attr->handle, Bsp_kAxisDataTypeAcce, attr->acce_data, sizeof(attr->acce_data) / sizeof(attr->acce_data[0]));
    Bsp_AxisSetDataDepth(attr->handle, Bsp_kAxisDataTypeGyro, attr->gyro_data, sizeof(attr->gyro_data) / sizeof(attr->gyro_data[0]));
    Bsp_AxisSetDataDepth(attr->handle, Bsp_kAxisDataTypeAngle, attr->angle_data, sizeof(attr->angle_data) / sizeof(attr->angle_data[0]));
    Bsp_AxisOpen(attr->handle);
    Bsp_AxisCalibrate(attr->handle);
}

static void AxisDemo_TaskOnStop(Osc_TTask *self)
{
    AxisDemo_TAttr* attr = &g_axis_demo_attr_;

    Bsp_AxisClose(attr->handle);
    Bsp_AxisFree(attr->handle);
}

static UInt32 AxisDemo_TaskOnRunOnce(Osc_TTask *self)
{
    AxisDemo_TAttr* attr = &g_axis_demo_attr_;
    volatile Int32 count;

    Bsp_TAxisAcceData acce_data[10];
    Bsp_TAxisGyroData gyro_data[10];
    Bsp_TAxisAngleData angle_data[10];
    
    Bsp_AxisRunOnce(attr->handle);
    
    Bsp_AxisGetData(attr->handle, Bsp_kAxisDataTypeAcce, acce_data, sizeof(acce_data) / sizeof(acce_data[0]));
    Bsp_AxisGetData(attr->handle, Bsp_kAxisDataTypeGyro, gyro_data, sizeof(gyro_data) / sizeof(gyro_data[0]));
    count = Bsp_AxisGetData(attr->handle, Bsp_kAxisDataTypeAngle, angle_data, sizeof(angle_data) / sizeof(angle_data[0]));
    
    return 10;
}

void AxisDemo_Init(void)
{
    Osc_TApplicationInitialize(&g_osc_application);
    
    if (Osc_TTaskCreate(&g_axis_demo_task_, &g_axis_demo_task_attr_) >= 0)
    {
        g_axis_demo_task_.OnStart = AxisDemo_TaskOnStart;
        g_axis_demo_task_.OnRunOnce = AxisDemo_TaskOnRunOnce;
        g_axis_demo_task_.OnTerminate = AxisDemo_TaskOnStop;

        Osc_TApplicationSetMainTask(&g_osc_application, &g_axis_demo_task_);
        Osc_TApplicationRun(&g_osc_application);
    }

    while (1);

}
